#include <iostream>
#include <string.h>
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/JointState.h>


int main(int argc, char **argv)
{
    ros::init(argc, argv, "sg805_pose_publisher");
    ros::NodeHandle nh;

    geometry_msgs::PoseStamped msg;
    msg.header.frame_id = "/base_link";

    tf::TransformListener listener;
    tf::StampedTransform transform;

    //发布话题"sg805/pose"
    ros::Publisher pose_publisher = 
        nh.advertise<geometry_msgs::PoseStamped>("/sg805/pose", 10);

    // 等待关节话题发布后，再开始执行
    // ros::topic::waitForMessage<sensor_msgs::JointState>("/sg805/joint_states");
    ros::topic::waitForMessage<sensor_msgs::JointState>("/joint_states");


    while (nh.ok())
    {
        try
        {   
            //监听link7相对于base_link的坐标变换
            listener.lookupTransform("/link1", "/link7",
                                     ros::Time(0), transform);
        }
        catch (tf::TransformException &ex)
        {
            ROS_ERROR("%s", ex.what());
            sleep(1);
            continue;
        }

        //保存至新的消息中
        msg.pose.position.x = transform.getOrigin().x();
        msg.pose.position.y = transform.getOrigin().y();
        msg.pose.position.z = transform.getOrigin().z();

        msg.pose.orientation.x = transform.getRotation().x();
        msg.pose.orientation.y = transform.getRotation().y();
        msg.pose.orientation.z = transform.getRotation().z();
        msg.pose.orientation.w = transform.getRotation().w();

        msg.header.stamp = ros::Time::now();

        //发布话题
        pose_publisher.publish(msg);

        //0.1s/次
        usleep(100000);
    }
    return 0;
};